/*
 * contourAffinage.cpp
 *
 *  Created on: 3 mars 2014
 *      Author: pierre
 */

#include "contourAffinage.h"

using namespace std;
using namespace cv;


std::vector<std::vector<cv::Point> > contourAffinage::approxPoly(std::vector<std::vector<cv::Point> > cont)
{
	vector<vector<Point> > contours_poly( cont.size() );
	vector<vector<Point> > contoursAfine;

	for( int i = 0; i < (int)cont.size(); i++ )
	{
		approxPolyDP(Mat(cont[i]), contours_poly[i], longProprePoly, true);

		vector<Point> cont;
		vector<Point> contMax;
		bool contMaxInit = false;

		if(contours_poly[i].size() >= 3)
		{
			float erreur = 0;
			int nbAngles = 0;
			int nbAngleDroit = 0;


			for(int k = 0; k < (int)contours_poly[i].size() - 2; k++)
			{
				int a_2 = pow( contours_poly[i][k+2].x - contours_poly[i][k+1].x, 2) +  pow( contours_poly[i][k+2].y - contours_poly[i][k+1].y, 2);
				int b_2 = pow( contours_poly[i][k+2].x - contours_poly[i][k].x, 2) +  pow( contours_poly[i][k+2].y - contours_poly[i][k].y, 2);
				int c_2 = pow( contours_poly[i][k+1].x - contours_poly[i][k].x, 2) +  pow( contours_poly[i][k+1].y - contours_poly[i][k].y, 2);

				float angleAK = 0;
				float rapport = (b_2-a_2-c_2)/(-2*pow(a_2, 0.5)*pow(c_2, 0.5));

				if(abs(rapport) < 1)
				{
					angleAK = acos(rapport);
				}

				//cout << "-> " << angleAK << endl;

				if(angleAK > angleRupture)
				{
					cont.push_back( contours_poly[i][k]);

					if(k == (int)(contours_poly[i].size() - 3) )
					{
						cont.push_back( contours_poly[i][k+1]);
						cont.push_back( contours_poly[i][k+2]);

						if(!contMaxInit || cont.size() > contMax.size())
						{
							contMaxInit = true;
							contMax = cont;
						}
					}
				}
				else
				{
					cont.push_back( contours_poly[i][k]);
					cont.push_back( contours_poly[i][k+1]);

					if(!contMaxInit || cont.size() > contMax.size())
					{
						contMaxInit = true;
						contMax = cont;
					}

					cont.clear();

					if(k == (int)(contours_poly[i].size() - 3) )
					{
						cont.push_back( contours_poly[i][k+1]);
						cont.push_back( contours_poly[i][k+2]);

						if(!contMaxInit || cont.size() > contMax.size())
						{
							contMaxInit = true;
							contMax = cont;
						}
					}
				}

				erreur += abs(angleAK - 1.55);
				nbAngles++;

				if(abs(angleAK - 1.55) < distanceAD)
				{
					nbAngleDroit++;
				}
			}

			contoursAfine.push_back(contMax);

			/*
			Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
			for( int j = 0; j < (int)contMax.size()-1; j++ )
			{
				line(drawing, contMax[j], contMax[j+1], color, 1);
				cvWaitKey(0);
				imshow( "GeckoGeek Window", drawing);
			}
			imshow( "GeckoGeek Window", drawing);
			cvWaitKey(0);
			*/


			//cout << "afinage: " << contMax.size()  << "/" << contours_poly[i].size() << endl;
			//cout << "erreur moy: " << erreur/nbAngles << endl;
			//cout << "ratio AngleDroit: " << (float)nbAngleDroit/nbAngles << endl;
		}
		else
		{
			if(contours_poly[i].size() > 1)
			{
				contoursAfine.push_back(contours_poly[i]);
			}
		}
	}

	return contoursAfine;
}

std::vector<std::vector<cv::Point> > contourAffinage::raccord(std::vector<std::vector<cv::Point> > cont)
{
	for(int i = 0; i < (int)cont.size(); i++)
	{
		for(int j = i+1; j < (int)cont.size(); j++)
		{
			int d1 = abs(cont[i][0].x -  cont[j][0].x) + abs(cont[i][0].y -  cont[j][0].y);
			int d2 = abs(cont[i][0].x -  cont[j].back().x) + abs( cont[i][0].y -  cont[j].back().y );
			int d3 = abs(cont[i].back().x -  cont[j][0].x) + abs(cont[i].back().y -  cont[j][0].y );
			int d4 = abs(cont[i].back().x -  cont[j].back().x) + abs(cont[i].back().y -  cont[j].back().y);

			bool raccord = false;

			if(d1 < distanceRaccord)
			{
				reverse(cont[j].begin(), cont[j].end());

				int newx = (cont[i][0].x +  cont[j].back().x) / 2;
				int newy = (cont[i][0].y +  cont[j].back().y) / 2;

				cont[i][0].x = newx;
				cont[i][0].y = newy;
				cont[j].erase( cont[j].end() );

				cont[j].insert( cont[j].end(), cont[i].begin(), cont[i].end() );

				raccord = true;
			}


			if(d2 < distanceRaccord && !raccord )
			{
				int newx = (cont[i][0].x +  cont[j].back().x) / 2;
				int newy = (cont[i][0].y +  cont[j].back().y) / 2;

				cont[i][0].x = newx;
				cont[i][0].y = newy;
				cont[j].erase( cont[j].end() );

				cont[j].insert( cont[j].end(), cont[i].begin(), cont[i].end() );

				raccord = true;
			}

			if(d3 < distanceRaccord && !raccord)
			{
				reverse(cont[j].begin(), cont[j].end());
				reverse(cont[i].begin(), cont[i].end());

				int newx = (cont[i][0].x +  cont[j].back().x) / 2;
				int newy = (cont[i][0].y +  cont[j].back().y) / 2;

				cont[i][0].x = newx;
				cont[i][0].y = newy;
				cont[j].erase( cont[j].end() );

				cont[j].insert( cont[j].end(), cont[i].begin(), cont[i].end() );

				raccord = true;
			}

			if(d4 < distanceRaccord && !raccord)
			{
				reverse(cont[i].begin(), cont[i].end());

				int newx = (cont[i][0].x +  cont[j].back().x) / 2;
				int newy = (cont[i][0].y +  cont[j].back().y) / 2;

				cont[i][0].x = newx;
				cont[i][0].y = newy;
				cont[j].erase( cont[j].end() );

				cont[j].insert( cont[j].end(), cont[i].begin(), cont[i].end() );

				raccord = true;
			}

			int d_extrem = abs(cont[j][0].x -  cont[j].back().x) + abs(cont[j][0].y -  cont[j].back().y);

			if(d_extrem < distanceRaccord)
			{
				int newx = (cont[j][0].x +  cont[j].back().x) / 2;
				int newy = (cont[j][0].y +  cont[j].back().y) / 2;

				cont[j][0].x = newx;
				cont[j][0].y = newy;
				cont[j].back().x= newx;
				cont[j].back().y= newy;
			}


			if(d1 < distanceRaccord || d2 < distanceRaccord || d3 < distanceRaccord || d4 < distanceRaccord)
			{
				cont.erase( cont.begin() + i );
				i--;
				break;
			}
		}
	}

	return cont;
}


vector<Contour > contourAffinage::calculeTauxAD(std::vector<std::vector<cv::Point> > cont)
{
	vector<Contour > listeCt;

	for(int i = 0; i < (int)cont.size(); i++)
	{
		Contour cts;

		if(cont[i].size() >= 3)
		{
			int nbAngles = 0;
			int nbAngleDroit = 0;

			for(int k = 0; k < (int)cont[i].size() - 2; k++)
			{
				int a_2 = pow( cont[i][k+2].x - cont[i][k+1].x, 2) +  pow( cont[i][k+2].y - cont[i][k+1].y, 2);
				int b_2 = pow( cont[i][k+2].x - cont[i][k].x, 2) +  pow( cont[i][k+2].y - cont[i][k].y, 2);
				int c_2 = pow( cont[i][k+1].x - cont[i][k].x, 2) +  pow( cont[i][k+1].y - cont[i][k].y, 2);

				float angleAK = 0;
				float rapport = (b_2-a_2-c_2)/(-2*pow(a_2, 0.5)*pow(c_2, 0.5));

				if(abs(rapport) < 1)
				{
					angleAK = acos(rapport);
				}

				if(angleAK > 2.5)
				{
					cont[i].erase(cont[i].begin()+k+1);
					k--;
				}
				else
				{
					nbAngles++;

					if(abs(angleAK - 1.55) < distanceAD)
					{
						nbAngleDroit++;
					}
				}
			}

			if(nbAngles != 0)
			{
				cts.id = i;
				cts.ratioAD = (float)nbAngleDroit/nbAngles;

				listeCt.push_back(cts);
			}
		}
	 }

	return listeCt;
}

/*
for(int k = 0; k < (int)contCroix.size()-2; k++)
{
	int a_2 = pow( contCroix[k+2].x - contCroix[k+1].x, 2) +  pow( contCroix[k+2].y - contCroix[k+1].y, 2);
	int b_2 = pow( contCroix[k+2].x - contCroix[k].x, 2) +  pow( contCroix[k+2].y - contCroix[k].y, 2);
	int c_2 = pow( contCroix[k+1].x - contCroix[k].x, 2) +  pow( contCroix[k+1].y - contCroix[k].y, 2);

	float angleAK = 0;
	float rapport = (b_2-a_2-c_2)/(-2*pow(a_2, 0.5)*pow(c_2, 0.5));

	if(abs(rapport) < 1)
	{
		angleAK = acos(rapport);
	}

	int distance  = pow(c_2, 0.5);

	//cout << "angle " << k << ": " << angleAK << " longeur: " << distance << endl;
}
*/

/*
	Mat drawing = Mat::zeros( image.size(), CV_8UC3 );
for( int i = 0; i < (int)contoursAfine.size(); i++ )
{
	Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );

	for( int j = 0; j < (int)contoursAfine[i].size()-1; j++ )
	{
		line(drawing, contoursAfine[i][j], contoursAfine[i][j+1], color, 1);
	}
}
*/

